#include <stdio.h>
#include <time.h>

/**
 * 高性能PID算法实现
 * 采用位置式PID算法，结合微分滤波和积分限幅
 * 适用于高频系统和实时控制
 */

// PID控制器结构体
typedef struct
{
    float Kp;           // 比例系数：决定系统相应速度
    float Ki;           // 积分系数：消除稳态误差
    float Kd;           // 微分系数：抑制超调
    float T;            // 采样周期(秒)：必须与实际控制周期一致
    float integral;     // 积分累积值
    float prev_error;   // 上一次误差
    float prev_measure; // 上一次测量值(用于微分滤波)
    float out_max;      // 输出上限
    float out_min;      // 输出下限
    float alpha;        // 微分滤波系数(0-1)，越大滤波效果越弱
} PIDController;

// PID初始化
void PID_Init(PIDController *pid)
{
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->prev_measure = 0.0f;
}

// 限幅函数
static float clamp(float value, float min, float max)
{
    return (value < min) ? min : (value > max) ? max
                                               : value;
}

// PID计算(位置式算法)
float PID_Compute(PIDController *pid, float setpoint, float measurement)
{
    // 计算误差
    float error = setpoint - measurement;

    // 比例项
    float proportional = pid->Kp * error;

    // 积分项(带抗饱和)
    pid->integral += pid->Ki * error * pid->T;
    pid->integral = clamp(pid->integral, pid->out_min, pid->out_max);

    // 微分项(带滤波)
    float derivative = (measurement - pid->prev_measure) / pid->T; // 测量值微分
    derivative = pid->alpha * derivative + (1 - pid->alpha) * pid->prev_error;
    derivative = pid->Kd * derivative;

    // 计算总输出
    float output = proportional + pid->integral - derivative; // 注意微分项符号

    // 输出限幅
    output = clamp(output, pid->out_min, pid->out_max);

    // 保存状态
    pid->prev_error = error;
    pid->prev_measure = measurement;

    return output;
}

// 示例测试
int main()
{
    PIDController pid = {
        .Kp = 4.0f, // 比例系数
        .Ki = 0.5f, // 积分系数
        .Kd = 2.0f, // 微分系数
        .T = 0.01f, // 10ms采样周期
        .out_max = 100.0f,
        .out_min = 0.0f,
        .alpha = 0.7f // 微分滤波系数
    };
    PID_Init(&pid);

    float setpoint = 100.0f;  // 目标温度
    float measurement = 0.0f; // 当前温度
    float output = 0.0f;

    // 模拟60次控制周期
    for (int i = 0; i < 60; i++)
    {
        output = PID_Compute(&pid, setpoint, measurement);

        // 模拟被控对象（一阶惯性环节）
        measurement += output * 0.1f; // 假设系统增益0.1

        printf("[%2d] Setpoint: %.1f°C, Measurement: %.1f°C, Output: %.1f%%\n",
               i + 1, setpoint, measurement, output);
    }

    return 0;
}